#ifndef MATRIXDECOMPOSITION_HPP
#define MATRIXDECOMPOSITION_HPP

#include <cmath>
#include "matrix.hpp"
#include "newmat10/newmat.h"
#include "newmat10/newmatap.h"
#include "newmat10/include.h" 
namespace TermStructure {

    //! LU Decomposition.
	template <typename TFromMatrix, typename TToMatrix>
	void LUDecomposeTo(const TFromMatrix &input, TToMatrix &L, TToMatrix &U)
	{
		long i = 0, j = 0, k = 0;
		long totalI = input.GetLength(0), totolJ = input.GetLength(1);
		for (; i < totalI; i++)
		{
			j = i;
			//Update the Upper one
			for (; j < totolJ; j++)
			{
				U(i, j) = input(i, j);
				k = 0;
				for (; k < i; k++)
				{
					U(i, j) = U(i, j) - L(i, k) * U(k, j);
				}
			}
			//Update the Lower one
			j = i + 1;
			for (; j < totalI; j++)
			{
				L(j, i) = input(j, i);
				k = 0;
				for (; k < i; k++)
				{
					L(j, i) = L(j, i) - L(j, k) * U(k, i);
				}
				L(j, i) = L(j, i) / U(i, i);
			}
			L(i,i) = 1;
		}
	}
    //! LU Decomposition. The L and U matrix is put into result, assuming diagonal elements of L is 1
	template<typename TFromMatrix, typename TToMatrix>
	void LUDecomposeTo(const TFromMatrix &input, TToMatrix &result)
	{
		TToMatrix &L = result;
		TToMatrix &U = result;
		long i = 0, j = 0, k = 0;
		long totalI = input.GetLength(0), totolJ = input.GetLength(1);
		for (; i < totalI; i++)
		{
			j = i;
			//Update the Upper one
			for (; j < totolJ; j++)
			{
				U(i, j) = input(i, j);
				k = 0;
				for (; k < i; k++)
				{
					U(i, j) = U(i, j) - L(i, k) * U(k, j);
				}
			}
			//Update the Lower one
			j = i + 1;
			for (; j < totalI; j++)
			{
				L(j, i) = input(j, i);
				k = 0;
				for (; k < i; k++)
				{
					L(j, i) = L(j, i) - L(j, k) * U(k, i);
				}
				L(j, i) = L(j, i) / U(i, i);
			}
		}
	}
    //! Cholesky Decomposition
	template<typename TFromMatrix, typename TToMatrix>
	void CholeskyDecomposeTo(const TFromMatrix &a, TToMatrix &b)
	{
		b(0,0)=std::sqrt(a(0,0));
		for (long i=0;i<a.GetLength(0);i++)
		{
			b(i,i)=a(i,i);				
			for(long j=0; j<i;j++)
			{
				b(i,i)-=b(i,j)*b(i,j);
			}
			b(i,i)=std::sqrt(b(i,i));
			for(long j=i+1;j<a.GetLength(0);j++)
			{
				b(j,i)=a(j,i);
				for (long k=0; k<i;k++)
				{
					b(j,i)=b(j,i)-b(j,k)*b(i,k);
				}
				b(j,i)=b(j,i)/b(i,i);
			}
		}
	}
    //! Singular Value Decomposition
	template <typename TFromMatrix, typename TToMatrix>
	void SingularValueDecomposeTo(const TFromMatrix &a, TToMatrix &u, TToMatrix &d, TToMatrix &v)
	{
		Matrix aa(a.GetLength(0),a.GetLength(1));
		for (long i=0;i<a.GetLength(0);i++)
		{
			for (long j=0;j<a.GetLength(1);j++)
			{
				aa(i+1,j+1)=a(i,j);
			}
		}
	    Matrix uu(a.GetLength(0),a.GetLength(1));
	    DiagonalMatrix dd(a.GetLength(0));
	    Matrix vv(a.GetLength(0),a.GetLength(1));
		SVD(aa,dd,uu,vv,true,true);
		for (long i=0;i<a.GetLength(0);i++)
		{
			for(long j=0;j<a.GetLength(1);j++)
			{
				u(i,j)=uu(i+1,j+1);
				v(i,j)=vv(i+1,j+1);
				if (i==j)
				{
					d(i,j)=dd(i+1);
				}
				else
				{
					d(i,j)=0;
				}
			}
		}
	}

	template <typename TInputMatrix, typename TLoadMatrix, typename TResultMatrix, typename TTempMatrix>
	void LUDecompositionSolve(const TInputMatrix &input, const TLoadMatrix &load, TResultMatrix &result, TTempMatrix &tempStorage)
	{
		LUDecomposeTo(input, tempStorage);
		for (long i = 0; i < tempStorage.GetLength(0); i++)
		{
			result(i, 0) = load(i, 0);
			for (long k = 0; k < i; k++)
			{
				result(i, 0) = result(i, 0) - result(k, 0) * tempStorage(i, k);
			}
		}
		for (long i = tempStorage.GetLength(0) - 1; i >= 0; i--)
		{
			for (long k = tempStorage.GetLength(0) - 1; k > i; k--)
			{
				result(i, 0) = result(i, 0) - result(k, 0) * tempStorage(i, k);
			}
			result(i, 0) = result(i, 0) / tempStorage(i, i);
		}
		return;
	}
}

#endif
